#include <Servo.h>
Servo S1;
Servo S2;
int S1max=180,S1min=23;//maksymalny i minimalny kąt 1 serva
int S2max=180,S2min=0;//maksymalny i minimalny kąt 2 serva 
int a=167,b=145,c=53,e=85;//wymiary w mm

void check(double,double,double);//funkcja liczy kąty jakie potrzebujem dla serva1 i serva2


void setup() {
  Serial.begin(9600);
  pinMode(A0,INPUT_PULLUP);
  S1.attach(9);//tuuuuuu
  S2.attach(10);//tuuuuuu
  S2.write(90);
  S1.write(90);
  delay(10);
}
 
void loop() {
  if(digitalRead(A0)==0)
  {
    int i = S2.read();
    int j = S1.read();

    double katServ[2];
    check(150,20,katServ);
    if(i<katServ[1]){
     for(i;i<=katServ[1];i++)
        {
        S2.write(i);
        delay(50);
        } //koniec fora 
        }
    if(i>katServ[1]){
     for(i;i<=katServ[1];i--)
        {
        S2.write(i);
        delay(50);
        } //koniec fora 
        }
    if(j<katServ[0]){
    for(j;j<=katServ[0];j++)
        {
        S1.write(j);
        delay(50);
        } //koniec fora
        }
    if(j>katServ[0]){
    for(j;j<=katServ[0];j--)
        {
        S1.write(j);
        delay(50);
        } //koniec fora
        }
   
  }//przycisk
 }


void check(double odlx,double wysy,double katServ[])
{
    int i,j,alfa,beta;
    double h,X,D,y1,y2,kat1,kat2,odl,roznica;
    roznica=100;
    

    for (i=S1min;i<S1max;i++)
    {
        for (j=S2min; j<S2max;j++)
        {   
            alfa=abs(i-90);
            kat1= (map(alfa,0,180,0,3147))/1000;//kąt w radianach
            if(i>=90)
            {   if((i+alfa)>180)
                beta=abs((j-90+alfa));
                else
                beta=j-180+alfa;
                
                kat2= (map(beta,0,180,0,3147))/1000;
            }
            else
            {   
                if(j<=alfa)
                beta=abs(alfa-j);
                else
                beta=abs((j-90-alfa));
                
                kat2= (map(beta,0,180,0,3147))/1000;
            }

            if(((i>=90) && (j+alfa>=180)) || ((i<90) && (j<=alfa)))
              {
                D=cos(kat2)*c;
                y2=sin(kat2)*c;
                X=cos(kat1)*b;
                y1=sin(kat1)*b;
              }
            else
              {
                X=cos(kat1)*b;
                D=sin(kat2)*c;
                y1=sin(kat1)*b;
                y2=cos(kat2)*c;
              }

            if((i>=90) && (j+alfa>=180))
            h=a+y1+y2-e;
            else if((i>=90) && (j+alfa<180))
            h=a+y1-y2-e;
            else if((i<90) && (j<=alfa))
            h=a-y1+y2-e;
            else if((i<90) && (j>alfa))
            h=a-y1-y2-e;

            if(((j>=(90-alfa)) && (i>=90)) || ((i<90) &&  (j>=(90+alfa))))
            odl=X+D;
            else
            odl=X-D;

            
            if (roznica<=(abs(odlx-odl)+wysy-h) && (wysy<=h))
            {
                roznica=(abs(odlx-odl)+wysy-h);//wazne
                katServ[0]=i;
                katServ[1]=180-j;
            }

        }
    }

}//koniec check